/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_MODELPOSESENSORMEASUREMENT_H_
#define __MMM_MODELPOSESENSORMEASUREMENT_H_

#include "../../SensorMeasurement.h"
#include "../../InterpolatableSensorMeasurement.h"

#include <Eigen/Core>

namespace MMM
{
class ModelPoseSensorMeasurement;

typedef boost::shared_ptr<ModelPoseSensorMeasurement> ModelPoseSensorMeasurementPtr;

class ModelPoseSensorMeasurement : public InterpolatableSensorMeasurement<ModelPoseSensorMeasurement>, SMCloneable<ModelPoseSensorMeasurement>
{

public:
    ModelPoseSensorMeasurement(float timestep, const Eigen::Vector3f &rootPosition, const Eigen::Vector3f &rootRotation, SensorMeasurementType type = SensorMeasurementType::MEASURED);

    ModelPoseSensorMeasurement(float timestep, const Eigen::Matrix4f &rootPose, SensorMeasurementType type = SensorMeasurementType::MEASURED);

    SensorMeasurementPtr clone();

    bool equals(SensorMeasurementPtr sensorMeasurement);

    ModelPoseSensorMeasurementPtr clone(float newTimestep);

    Eigen::Vector3f getRootPosition();

    Eigen::Vector3f getRootRotation();

    Eigen::Matrix4f getRootPose();

protected:
    ModelPoseSensorMeasurementPtr interpolate(ModelPoseSensorMeasurementPtr other, float timestep, SensorMeasurementType type);

    void appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode);

private:
    Eigen::Vector3f rootPosition;
    Eigen::Vector3f rootRotation;
};
}
#endif // __MMM_MODELPOSESENSORMEASUREMENT_H_
